#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
#xbox
import os, struct, array
from fcntl import ioctl
#teleop_twist
import sys
import geometry_msgs.msg

# We'll store the states here.
axis_states = {
    'LX': 0,
    'LY': 0,
    'RX': 0,
    'RY': 0,
    'LT': 0,
    'RT': 0,
    'XX': 0,
    'YY': 0,
}
button_states = {
    'A': 0,
    'B': 0,
    'X': 0,
    'Y': 0,
    'LB': 0,
    'RB': 0,
    'START': 0,
    'BACK': 0,
    'HOME': 0,
    'LO': 0,
    'RO': 0,
}

# 先前校验时，方向盘是x,左侧踏板是z,右侧踏板是rz。


XBOX_TYPE_BUTTON = 0x01
XBOX_TYPE_AXIS = 0x02

XBOX_BUTTON_A = 0x00
XBOX_BUTTON_B = 0x01
XBOX_BUTTON_X = 0x02
XBOX_BUTTON_Y = 0x03
XBOX_BUTTON_LB = 0x04
XBOX_BUTTON_RB = 0x05
XBOX_BUTTON_START = 0x06
XBOX_BUTTON_BACK = 0x07
XBOX_BUTTON_HOME = 0x08
XBOX_BUTTON_LO = 0x09    # /* 左摇杆按键 */
XBOX_BUTTON_RO = 0x0a    # /* 右摇杆按键 */

XBOX_BUTTON_ON = 0x01
XBOX_BUTTON_OFF = 0x00

XBOX_AXIS_LX = 0x00   # /* 左摇杆X轴 */
XBOX_AXIS_LY = 0x01   # /* 左摇杆Y轴 */
XBOX_AXIS_RX = 0x03   # /* 右摇杆X轴 */
XBOX_AXIS_RY = 0x04   # /* 右摇杆Y轴 */
XBOX_AXIS_LT = 0x02
XBOX_AXIS_RT = 0x05
XBOX_AXIS_XX = 0x06    # /* 方向键X轴 */
XBOX_AXIS_YY = 0x07    # /* 方向键Y轴 */

XBOX_AXIS_VAL_UP = -32767
XBOX_AXIS_VAL_DOWN = 32767
XBOX_AXIS_VAL_LEFT = -32767
XBOX_AXIS_VAL_RIGHT = 32767

XBOX_AXIS_VAL_MIN = -32767
XBOX_AXIS_VAL_MAX = 32767
XBOX_AXIS_VAL_MID = 0x00

fn = '/dev/input/js1'


axis_map = []
axis_names = {
    0x00: 'x',
    0x02: 'z',
    0x05: 'rz',
}
# Open the joystick device.


moveBindings = {
    'i': (1, 0, 0, 0),
    'o': (1, 0, 0, -1),
    'j': (0, 0, 0, 1),
    'l': (0, 0, 0, -1),
    'u': (1, 0, 0, 1),
    ',': (-1, 0, 0, 0),
    '.': (-1, 0, 0, 1),
    'm': (-1, 0, 0, -1),
    'O': (1, -1, 0, 0),
    'I': (1, 0, 0, 0),
    'J': (0, 1, 0, 0),
    'L': (0, -1, 0, 0),
    'U': (1, 1, 0, 0),
    '<': (-1, 0, 0, 0),
    '>': (-1, -1, 0, 0),
    'M': (-1, 1, 0, 0),
    't': (0, 0, 1, 0),
    'b': (0, 0, -1, 0),
}

speed = 0.5
turn = 1.0
x = 0.0
y = 0.0
z = 0.0
th = 0.0
status = 0.0

class xbox_publisher(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("hello world,my name is %s!" % name)

        # 创建订阅者
        self.command_publisher_ = self.create_publisher(geometry_msgs.msg.Twist, 'cmd_vel', 10)

        fn = '/dev/input/js1'
        self.key = 'k'

        self.jsdev = open(fn, 'rb')

        # # Get the device name.
        buf = array.array('u', ['\0'] * 5)
        ioctl(self.jsdev, 0x80006a13 + (0x10000 * len(buf)), buf)  # JSIOCGNAME(len)
        js_name = buf.tobytes()
        print('Device name: %s' % js_name)

        # Get number of axes and buttons.
        buf = array.array('B', [0])
        ioctl(self.jsdev, 0x80016a11, buf)  # JSIOCGAXES
        num_axes = buf[0]

        # Get the axis map.
        buf = array.array('B', [0] * 0x40)
        ioctl(self.jsdev, 0x80406a32, buf)  # JSIOCGAXMAP

    def xbox_read(self):
        while True:# Main event loop
            evbuf = self.jsdev.read(8)
            if evbuf:
                time, value, type, number = struct.unpack('IhBB', evbuf)
                if type & 0x01:
                    if number == XBOX_BUTTON_A:
                        button_states["A"] = value
                    elif number == XBOX_BUTTON_B:
                        button_states["B"] = value
                    elif number == XBOX_BUTTON_X:
                        button_states["X"] = value
                    elif number == XBOX_BUTTON_Y:
                        button_states["Y"] = value
                    elif number == XBOX_BUTTON_LB:
                        button_states["LB"] = value
                    elif number == XBOX_BUTTON_RB:
                        button_states["RB"] = value
                    elif number == XBOX_BUTTON_START:
                        button_states["START"] = value
                    elif number == XBOX_BUTTON_BACK:
                        button_states["BACK"] = value
                    elif number == XBOX_BUTTON_HOME:
                        button_states["HOME"] = value
                    elif number == XBOX_BUTTON_LO:
                        button_states["LO"] = value
                    elif number == XBOX_BUTTON_RO:
                        button_states["RO"] = value
                    #print(button_states)
                elif type & 0x02:
                    if number == XBOX_AXIS_LX:
                        axis_states["LX"] = value
                    elif number == XBOX_AXIS_LY:
                        axis_states["LY"] = value
                    elif number == XBOX_AXIS_RX:
                        axis_states["RX"] = value
                    elif number == XBOX_AXIS_RY:
                        axis_states["RY"] = value
                    elif number == XBOX_AXIS_LT:
                        axis_states["LT"] = value
                    elif number == XBOX_AXIS_RT:
                        axis_states["RT"] = value
                    elif number == XBOX_AXIS_XX:
                        axis_states["XX"] = value
                    elif number == XBOX_AXIS_YY:
                        axis_states["YY"] = value
                    #print(axis_states)
                if(button_states["RB"] == 1):
                    print("RB is on")
                    self.key = 'k'

                    if(axis_states["XX"] > 0):
                        print("right is on")
                        self.key = 'l'
                    elif (axis_states["XX"] < 0):
                        print("left is on")
                        self.key = 'j'
                    
                    if(axis_states["YY"] > 0):
                        print("after is on")
                        self.key = ','
                    elif (axis_states["YY"] < 0):
                        print("front is on")
                        self.key = 'i'

                    if self.key in moveBindings.keys():
                        x = moveBindings[self.key][0]
                        y = moveBindings[self.key][1]
                        z = moveBindings[self.key][2]
                        th = moveBindings[self.key][3]
                    else:
                        x = 0.0
                        y = 0.0
                        z = 0.0
                        th = 0.0

                else:
                    x = 0.0
                    y = 0.0
                    z = 0.0
                    th = 0.0
                    print("RB is off")

                twist = geometry_msgs.msg.Twist()
                twist.linear.x = x * speed
                twist.linear.y = y * speed
                twist.linear.z = z * speed
                twist.angular.x = 0.0
                twist.angular.y = 0.0
                twist.angular.z = th * turn
                self.command_publisher_.publish(twist)


def main(args=None):
    rclpy.init(args=args) # 初始化rclpy
    node = xbox_publisher("teleop_twist_xbox") # 新建一个节点
    node.xbox_read()
    rclpy.spin(node)# 保持节点运行，检测是否收到退出指令（Ctrl+C）
    rclpy.shutdown()# 关闭rclpy